// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.Cameras
{
    using System;
    using System.Collections.ObjectModel;
    using System.Drawing;
    using System.Runtime.Serialization;

    using Microsoft.Kinect;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Cameras.Kinect;

    /// <summary>
    /// Kinect producer agent publishing a depth data/message
    /// </summary>
    [DataContract]
    public class KinectAgent : ProducerAgent<DepthCameraAgentMessage<KinectDepthCameraParameters>>, 
        IMessagePublisher<ColorCameraAgentMessage<KinectColorCameraParameters>>,
        IMessagePublisher<IRCameraAgentMessage<KinectIRCameraParameters>>,
        IControlReceiver<KinectCameraControlMessage>
    {
        /// <summary>
        /// The ConnectKinect 
        /// </summary>
        private ConnectKinect connect;

        /// <summary>
        /// The type of depth image to publish. Default is Resolution640x480Fps30
        /// </summary>
        [DataMember(Name = "DepthImageFormat")]
        private DepthImageFormat? depthImageFormat;

        /// <summary>
        /// Images produced are left to right instead of kinect native right to left (default is true)
        /// </summary>
        [DataMember(Name = "leftRightImages")]
        private bool? leftRightImages;

        /// <summary>
        /// Maximum depth value supported in millimeters.
        /// Note that this can be different from the value reported by depthImageStream.MaxDepth.
        /// depthImageStream.MaxDepth was correct for the old API, but is incorrect for the new API (CopyDepthImagePixelDataTo).
        /// In order to support Maximum depth, we are explicitly limiting the depth data to this range.
        /// You can simulate old behavior by limiting this to 4000. (4m).
        /// Note that minimumRange has not been added for performance reasons.
        /// If there is any need for it, it can be easily added parallel to the maximum depth clamping.
        /// </summary>
        [DataMember(Name = "MaximumDepthImageRange")]
        private short maximumRange;

        /// <summary>
        /// Value indicating no reading
        /// </summary>
        [DataMember(Name = "NoReadingValue")]
        private short noReadingValue;

        /// <summary>
        /// Value indicating a reading above maximumRange
        /// </summary>
        [DataMember(Name = "TooFarValue")]
        private short tooFarValue;

        /// <summary>
        /// Value indicating a reading below minimumRange
        /// </summary>
        [DataMember(Name = "TooNearValue")]
        private short tooNearValue;

        /// <summary>
        /// Device robot-relative pose.
        /// </summary>
        [DataMember(Name = "Pose")]
        private Pose pose;

        /// <summary>
        /// Time Lag in milliseconds for generating a Kinect frame
        /// </summary>
        [DataMember(Name = "KinectMessageLag")]
        private long kinectMessageLag;

        /// <summary>
        /// The type of RGB image to publish. Undefined format disables RGB image collection
        /// </summary>
        [DataMember(Name = "RGBImageFormat")]
        private ColorImageFormat? rgbImageFormat;

        /// <summary>
        /// The intrinsics of the depth camera
        /// </summary>
        [DataMember]
        private Matrix3 depthIntrinsics;

        /// <summary>
        /// The intrinsics of the color camera
        /// </summary>
        [DataMember]
        private Matrix3 colorIntrinsics;

        /// <summary>
        /// The intrinsics for the IR camera
        /// </summary>
        [DataMember]
        private Matrix3 irIntrinsics;

        /// <summary>
        /// The alignment parameters
        /// </summary>
        private ReadOnlyCollection<byte> kinectParams;

        /// <summary>
        /// Forwards the color message
        /// </summary>
        private IForwarder<ColorCameraAgentMessage<KinectColorCameraParameters>> colorForwarder;

        /// <summary>
        /// Forwards the infrared message
        /// </summary>
        private IForwarder<IRCameraAgentMessage<KinectIRCameraParameters>> irForwarder;
        
        /// <summary>
        /// Initializes a new instance of the KinectAgent class.
        /// </summary>
        /// <param name="name">Name of the agent</param>
        /// <param name="depthImageFormat">format of the depth image</param>
        /// <param name="rgbImageFormat">format of the color image</param>
        /// <param name="pose">Pose of the kinect depth camera, default is 0 rotation and translation</param>
        /// <param name="leftRightImages">Images produced are left to right (default is true)</param>
        /// <param name="kinectMessageLag">The lag of the kinect</param>
        /// <param name="maximumRange">Maximum range of kinect sensor, if not set then will take parameters provided by Kinect</param>
        /// <param name="noReadingValue">The no reading value, if not set then will take parameters provided by Kinect</param>
        /// <param name="tooFarValue">Too far value, if not set then will take parameters provided by Kinect</param>
        /// <param name="tooNearValue">Too near value, if not set then will take parameters provided by Kinect</param>
        /// <param name="depthIntrinsics">The depth camera intrinsics</param>
        /// <param name="colorIntrinsics">The color camera intrinsics</param>
        /// <param name="irIntrinsics">The IR camera intrinsics</param>
        public KinectAgent(
            string name,
            DepthImageFormat depthImageFormat,
            ColorImageFormat rgbImageFormat,
            Pose pose = new Pose(),
            bool leftRightImages = true,
            long kinectMessageLag = 0,
            short maximumRange = -1,
            short noReadingValue = -1,
            short tooFarValue = -1,
            short tooNearValue = -1,
            Matrix3 depthIntrinsics = null,
            Matrix3 colorIntrinsics = null,
            Matrix3 irIntrinsics = null)
            : base(name)
        {
            this.pose = pose;
            this.maximumRange = maximumRange;
            this.depthImageFormat = depthImageFormat;
            this.rgbImageFormat = rgbImageFormat;
            this.noReadingValue = noReadingValue;
            this.tooFarValue = tooFarValue;
            this.tooNearValue = tooNearValue;
            this.kinectMessageLag = kinectMessageLag;
            this.leftRightImages = leftRightImages;
            this.depthIntrinsics = depthIntrinsics;
            this.colorIntrinsics = colorIntrinsics;
            this.irIntrinsics = irIntrinsics;
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            // set the depth image format to its default value if undefifined or not set
            if (this.depthImageFormat == null || this.depthImageFormat == DepthImageFormat.Undefined)
            {
                this.depthImageFormat = DepthImageFormat.Resolution640x480Fps30;
            }

            // RGB image collection was added at a later stage. To ensure it is turned off by default
            // the default image format is Undefined
            if (this.rgbImageFormat == null)
            {
                this.rgbImageFormat = ColorImageFormat.Undefined;
            }

            this.connect = new ConnectKinect(   
                depthFrameReady: this.PublishDepthFrame,
                colorFrameReady: this.PublishColorFrame,
                irFrameReady: this.PublishIrFrame,
                depthImageFormat: (DepthImageFormat)this.depthImageFormat,
                rgbImageFormat: (ColorImageFormat)this.rgbImageFormat,
                leftRightImages: this.leftRightImages.Value);

            this.kinectParams = this.connect.DetectAndConnectKinect();
        }

        /// <summary>
        /// Called when the agent is about to end
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();

            this.connect.DisconnectKinect();
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<ColorCameraAgentMessage<KinectColorCameraParameters>> forwarder)
        {
            this.colorForwarder = forwarder;
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<IRCameraAgentMessage<KinectIRCameraParameters>> forwarder)
        {
            this.irForwarder = forwarder;
        }

        /// <summary>
        /// Handles control message to set kinect camera state
        /// </summary>
        /// <param name="kinectControlMessage">Kinect camera control message</param>
        public void ReceiveControl(KinectCameraControlMessage kinectControlMessage)
        {
            if (null != kinectControlMessage.State)
            {
                this.connect.SetState(kinectControlMessage.State);
            }
        }

        /// <summary>
        /// Publishes a depth frame
        /// </summary>
        /// <param name="timestamp">The timestamp of the message</param>
        /// <param name="depthImageStream">The depth image parameters</param>
        /// <param name="depthFrame">The depth image</param>
        /// <param name="state">The state of the kinect</param>
        private void PublishDepthFrame(
            long timestamp,
            DepthImageStream depthImageStream, 
            ImageFrameDepth depthFrame,
            KinectCameraState state)
        {
            KinectDepthCameraParameters kinectParams = new KinectDepthCameraParameters(
                this.depthIntrinsics == null ? CameraParameters.CreateIntrinsics(depthImageStream.NominalFocalLengthInPixels, depthImageStream.NominalFocalLengthInPixels, depthFrame.Width / 2, depthFrame.Height / 2) : this.depthIntrinsics,
                new LensDistortion(),
                depthImageStream.NominalHorizontalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                depthImageStream.NominalVerticalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                depthImageStream.Format,
                this.maximumRange != -1 ? this.maximumRange : (short)depthImageStream.MaxDepth,
                (short)depthImageStream.MinDepth,
                depthImageStream.Range,
                this.tooFarValue != -1 ? this.tooFarValue : (short)depthImageStream.TooFarDepth,
                this.tooNearValue != -1 ? this.tooNearValue : (short)depthImageStream.TooNearDepth,
                this.noReadingValue != -1 ? this.noReadingValue : (short)depthImageStream.UnknownDepth,
                this.leftRightImages.Value,
                state,
                this.kinectParams);

            DepthCameraAgentMessage<KinectDepthCameraParameters> message = new DepthCameraAgentMessage<KinectDepthCameraParameters>(
                timestamp - this.kinectMessageLag,
                depthFrame,
                kinectParams);

            this.Publisher.Post(message);
        }

        /// <summary>
        /// Publishes a color image
        /// </summary>
        /// <param name="timestamp">The timestamp of the message</param>
        /// <param name="rgbImageStream">The color image parameters</param>
        /// <param name="rgbFrame">The color image</param>
        /// <param name="state">The state of the kinect</param>
        private void PublishColorFrame(
            long timestamp,
            ColorImageStream rgbImageStream,
            ImageFrameRgb rgbFrame,
            KinectCameraState state)
            {
            KinectColorCameraParameters colorParams = new KinectColorCameraParameters(
                this.colorIntrinsics == null ? CameraParameters.CreateIntrinsics(rgbImageStream.NominalFocalLengthInPixels, rgbImageStream.NominalFocalLengthInPixels, rgbFrame.Width / 2, rgbFrame.Height / 2) : this.colorIntrinsics,
                new LensDistortion(),
                rgbImageStream.NominalHorizontalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                rgbImageStream.NominalVerticalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                this.leftRightImages.Value,
                rgbImageStream,
                state);

            ColorCameraAgentMessage<KinectColorCameraParameters> message = new ColorCameraAgentMessage<KinectColorCameraParameters>(
                timestamp - this.kinectMessageLag,
                    rgbFrame,
                colorParams);

            this.colorForwarder.Post(message);
        }

        /// <summary>
        /// Publishes an infrared image
        /// </summary>
        /// <param name="timestamp">The timestamp of the message</param>
        /// <param name="irImageStream">The infrared image parameters</param>
        /// <param name="irFrame">the infrared image</param>
        /// <param name="state">The state of the kinect</param>
        private void PublishIrFrame(
            long timestamp,
            ColorImageStream irImageStream,
            ImageFrame<ushort> irFrame,
            KinectCameraState state)
        {
            KinectIRCameraParameters irParameters = new KinectIRCameraParameters(
                this.irIntrinsics == null ? CameraParameters.CreateIntrinsics(irImageStream.NominalFocalLengthInPixels, irImageStream.NominalFocalLengthInPixels, irFrame.Width / 2, irFrame.Height / 2) : this.irIntrinsics,
                new LensDistortion(),
                irImageStream.NominalHorizontalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                irImageStream.NominalVerticalFieldOfView * MathConstants.Degrees2Radians / 2.0f,
                this.leftRightImages.Value,
                irImageStream,
                state);

            IRCameraAgentMessage<KinectIRCameraParameters> message = new IRCameraAgentMessage<KinectIRCameraParameters>(
                timestamp - this.kinectMessageLag,
                    irFrame,
                irParameters);

            this.irForwarder.Post(message);
        }
    }
}
